#!/usr/bin/python
# -*- coding: UTF-8 -*-
import sys;
sys.path.append("..")
from lib.MiniBotControlLib import MiniBotControlLib
from lib.MiniBotAlgorithmLib import MiniBotAlgorithmLib
import time

class ManipulatorRobot:
    #ID 列表
    ids_size = 0
    ids = []

    #当前关节角度
    position_angles = []

    #当前空间坐标 XYZ Roll Pitch Yaw
    spatial_coordinate = []
  
    #控制Api
    algLib = MiniBotAlgorithmLib()
    #lib = MiniBotControlLib("udp_mode")

    #初始化机器人：使用 BwRobotLib 对象
    def __init__(self,robotlib):
        self.robotlib = robotlib
        self.lib = self.robotlib.mclib

    #绑定操作臂ID
    def bindIds(self,ids):
        #TODO:TEST-----
        #ids = [20, 39, 32,  29, 33, 41]

        size = len(ids)
        self.ids_size = size
        self.ids = [0] * size
        self.position_angles = [0] * size
        self.spatial_coordinate = [0] * 6

        for i in range(size):
            self.ids[i] = ids[i]

        pass

    #读取操作臂当前角度位置
    def readAngles(self):
        for i in range(self.ids_size):
          #设置位置模式
          self.lib.SetMotorMode(self.ids[i],0)
          #预留时间给设备响应
          time.sleep(0.05)
          #读取第i个关节当前位置
          self.position_angles[i] = self.lib.GetMotorPosition(self.ids[i])
          #预留时间给设备响应
          time.sleep(0.05)
          print("关节模块"+str(self.ids[i])+"位置："+str(self.position_angles[i]));

        #计算新的空间坐标
        self.spatial_coordinate = self.algLib.CalJointAnglesToSpatialCoordinate(self.position_angles,self.ids_size - 1,0)
    
    #设置角度位置
    def setJointAngles(self,angles):
        size = len(angles)
        idsLen = len(self.ids)

        #赋值到当前变量
        for i in range(size):
            if(i < idsLen):
                self.position_angles[i] = int(angles[i])

        #将当前变量设给硬件
        #self.lib.SetPositionSync(self.ids,self.position_angles) #仅37版以上舵机支持
        self.lib.SetIdsPositionsWithSpeed(self.ids,self.position_angles,18)   #速度值 36 与 37 单位不一致 36 30% 可能对应 37 2%
        #print("self.lib SetJointAngles...")

        #计算新的空间坐标
        self.spatial_coordinate = self.algLib.CalJointAnglesToSpatialCoordinate(self.position_angles,idsLen - 1,0)

    #设置空间坐标
    def setSpatialCoordinate(self,coordinate):
        size = len(coordinate)
        idsLen = len(self.ids)

        #空间坐标记录关节角
        angles = self.algLib.CalSpatialCoordinateToJointAngles(coordinate,idsLen - 1,0)

        #判断是否为有效值
        isValid = False
        for i in range(idsLen - 1):
            val = angles[i]
            if(val != 0):
                isValid = True
                break

        #如果有效，则设置到硬件
        if(isValid):
            self.setJointAngles(angles)
        pass

    #将角度值推到硬件
    def pushJointsAngles(self):
        self.setJointAngles(self.position_angles)

    #将空间坐标值推到硬件
    def pushSpatialCoordinate(self):
        self.setSpatialCoordinate(self.spatial_coordinate)

    #夹起
    #I T T I T (I) G
    #0 90 12 0 60 90
    #                      0
    #      -90
    def pickUpSample(self):
        print("拿起")
        #恢复初始状态
        self.position_angles = [0] * self.ids_size
        self.pushJointsAngles()
        time.sleep(3)

        #下探张开
        self.position_angles[1] = 90
        self.position_angles[2] = 12
        self.position_angles[4] = 60
        self.position_angles[self.ids_size - 1] = 90
        self.pushJointsAngles()
        time.sleep(3)

        #（下探）夹紧
        self.position_angles[self.ids_size - 1] = 0
        self.pushJointsAngles()
        time.sleep(3)

        #（下探）->抬起
        self.position_angles[2] = -90
        self.pushJointsAngles()
        time.sleep(3)
        pass

    #放下
    #I T T I T (I) G
    #0 90 -90 0 60 90
    #90 0
    #       12
    #90 90 30 0 60 90
    def putDownSample(self):
        print("放下")
        #转向
        self.position_angles[0] = 90
        self.pushJointsAngles()
        time.sleep(3)

        #下探
        self.position_angles[2] = 12
        self.pushJointsAngles()
        time.sleep(3)

        #（下探）->松开
        self.position_angles[self.ids_size - 1] = 90
        self.pushJointsAngles()
        time.sleep(3)

        #抬起初始状态
        self.position_angles = [0] * self.ids_size
        self.pushJointsAngles()
        time.sleep(3)
        pass
